#include "pose_correct.h"

using namespace std;
using namespace cv;

void PoseCorrect::correct(const cv::Mat& inMat, cv::Mat& outMat)
{
	if (inMat.empty()) {
		return;
	}

	Mat perspectiveMatrix;
	getProjectionMatrix(inMat, perspectiveMatrix);
	if (perspectiveMatrix.empty()) {
		perspectiveMatrix = Mat::eye(3, 3, CV_32F);
	}
	warpPerspective(inMat, outMat, perspectiveMatrix, inMat.size(), INTER_LINEAR, BORDER_REPLICATE);

#ifdef RUN_DEBUG
	imshow("before correct", inMat);
	imshow("after correct", outMat);
	waitKey(10);
#endif
}


bool PoseCorrect::findCorners(const cv::Mat& image, std::vector<cv::Point2f>& points)
{
	if (image.empty()) {
		return false;
	}

	bool found = findChessboardCorners(image, { 3, 3 }, points,
		CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
	if (!found) {
		return false;
	}

	Mat viewGray = image;
	if (viewGray.type() != CV_8UC1) {
		cvtColor(image, viewGray, COLOR_BGR2GRAY);
	}
	cornerSubPix(viewGray, points, Size(11, 11), Size(-1, -1),
		TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 30, 0.1));

	return true;
}


